/*
 * ClarkeAndParkTransformation.c
 *
 *  Created: 20251017
 *  Author: Hecate
 */

#include "ClarkeAndParkTransformation.h"

#include <math.h>

void clarkeTransformation(float ia, float ib, float ic, float *iAlpha, float *iBeta)
{
    *iAlpha = ia;
    *iBeta = 0.5773502692f * ia + 1.154700538f * ib;  // 1/sqrt(3) = 0.577..., 2/sqrt(3) = 1.154...
}

void inverseClarkeTransformation(float iAlpha, float iBeta, float *ia, float *ib, float *ic)
{
    *ia = iAlpha;
    *ib = -0.5f * iAlpha + 0.8660254038f * iBeta;  // -1/2 = -0.5, sqrt(3)/2 = 0.866...
    *ic = -0.5f * iAlpha - 0.8660254038f * iBeta;  // -1/2 = -0.5, -sqrt(3)/2 = -0.866...
}

void parkTransformation(float iAlpha, float iBeta, float *id, float *iq, float theta)
{
    *id = iAlpha * cosf(theta) + iBeta * sinf(theta);
    *iq = -iAlpha * sinf(theta) + iBeta * cosf(theta);
}

void inverseParkTransformation(float id, float iq, float *iAlpha, float *iBeta, float theta)
{
    *iAlpha = id * cosf(theta) - iq * sinf(theta);
    *iBeta = id * sinf(theta) + iq * cosf(theta);
}

void clarkeAndParkTransformation(float ia, float ib, float ic, float theta, float *id, float *iq)
{
    float iAlpha = 0.0f;
    float iBeta = 0.0f;
    clarkeTransformation(ia, ib, ic, &iAlpha, &iBeta);
    parkTransformation(iAlpha, iBeta, id, iq, theta);
}

void inverseClarkeAndParkTransformation(float id, float iq, float theta, float *ia, float *ib, float *ic)
{
    float iAlpha = 0.0f;
    float iBeta = 0.0f;
    inverseParkTransformation(id, iq, &iAlpha, &iBeta, theta);
    inverseClarkeTransformation(iAlpha, iBeta, ia, ib, ic);
}
